


#include "senser_cfg.h"
#include "main.h"
#include "abz_encoder.h"


TIM_HandleTypeDef shenser_htim;





/* TIMx init function */
static void abz_encoder_interface_init(void)
{
    TIM_Encoder_InitTypeDef sConfig = {0};
    TIM_MasterConfigTypeDef sMasterConfig = {0};
    GPIO_InitTypeDef GPIO_InitStruct = {0};

    /* GPIO Ports Clock Enable */
    __HAL_RCC_GPIOC_CLK_ENABLE();
    __HAL_RCC_GPIOB_CLK_ENABLE();
    __HAL_RCC_GPIOA_CLK_ENABLE();
    /* TIMX clock enable */
    SENSER_TIM_CLK_ENABLE();
  
    /*TIM CH1 for A*/
    GPIO_InitStruct.Pin = GPIO_MISO_A_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = TIMX_MOSI_A_ALTERNATE;
    HAL_GPIO_Init(GPIO_MISO_A_GPIO_Port, &GPIO_InitStruct);
    /*TIM CH2 for B*/
    GPIO_InitStruct.Pin = GPIO_MOSI_B_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
    GPIO_InitStruct.Alternate = TIMX_MOSI_B_ALTERNATE;
    HAL_GPIO_Init(GPIO_MOSI_B_GPIO_Port, &GPIO_InitStruct);
    /*EXTI for Z*/
    GPIO_InitStruct.Pin = GPIO_SCLK_Z_Pin;
    GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIO_SCLK_Z_GPIO_Port, &GPIO_InitStruct);
    
    shenser_htim.Instance = SENSER_TIM_INSTANCE;
    shenser_htim.Init.Prescaler = 0;
    shenser_htim.Init.CounterMode = TIM_COUNTERMODE_UP;
    shenser_htim.Init.Period = 4000;
    shenser_htim.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    shenser_htim.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
    sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
    sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
    sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
    sConfig.IC1Filter = 0;
    sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
    sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
    sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
    sConfig.IC2Filter = 0;
    if (HAL_TIM_Encoder_Init(&shenser_htim, &sConfig) != HAL_OK)
    {
        Error_Handler();
    }
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
    if (HAL_TIMEx_MasterConfigSynchronization(&shenser_htim, &sMasterConfig) != HAL_OK)
    {
        Error_Handler();
    }

    /* EXTI interrupt init*/
    HAL_NVIC_SetPriority(EXTI9_5_IRQn, 1, 0);
    HAL_NVIC_EnableIRQ(EXTI9_5_IRQn);
}


/*********************************************************************接口配置结束*****************************************************************/

void abz_encoder_init(void)
{
    abz_encoder_interface_init();

    /* TIM4 interrupt Init */
    /* if need
    HAL_NVIC_SetPriority(TIM3_IRQn, 2, 0);
    HAL_NVIC_EnableIRQ(TIM3_IRQn);
    __HAL_TIM_ENABLE_IT(&shenser_htim, TIM_IT_UPDATE);
    */
    // 启动编码器模式
    HAL_TIM_Encoder_Start(&shenser_htim, TIM_CHANNEL_ALL);
}


/*根据实际使用的EXTI通道修改*/
void EXTI9_5_IRQHandler(void)
{
    /* USER CODE BEGIN EXTI9_5_IRQn 0 */
//    if(shenser_htim.Instance->SR != 0x00)
//    {
//        shenser_htim.Instance->CNT = 0x00;
//    }
//    else
//    {
//        shenser_htim.Instance->CNT = 0x7FFF;
//    }
//    printf("abz:%d\r\n", shenser_htim.Instance->CNT);
//    if(shenser_htim.Instance->CNT > 2000)
//        shenser_htim.Instance->CNT = 0;
//    else
//        shenser_htim.Instance->CNT = 4000;
  /* USER CODE END EXTI9_5_IRQn 0 */
  HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5);
  /* USER CODE BEGIN EXTI9_5_IRQn 1 */

  /* USER CODE END EXTI9_5_IRQn 1 */
}

//float position = 0.0f;
//float rotation = 0.0f;
//static float angle = 0.0f;
//static float last_angle = 0.0f;

#define ABZ_PULSE_TO_ANGLE_GAIN   (360.0f / 4000.0f)  
void abz_encoder_update(void)
{
    uint16_t pulse_cnt = 0;
    
    pulse_cnt = shenser_htim.Instance->CNT;
    
    if(sys.cfg.motor_dir == 0)
        sys.foc_controller.angle = pulse_cnt * ABZ_PULSE_TO_ANGLE_GAIN;
    else
        sys.foc_controller.angle = 360.0f - pulse_cnt * ABZ_PULSE_TO_ANGLE_GAIN;
    
    /*sys.foc_controller.angle * _2PI / 360.0f * MOTOR_PP*/
    sys.foc_controller.ele_rad = sys.foc_controller.angle * sys.cfg.rad_gain - sys.cfg.rad_offset;

    while(sys.foc_controller.ele_rad > _2PI)
    {
        sys.foc_controller.ele_rad -= _2PI;
    }
    while(sys.foc_controller.ele_rad < 0.0f)
    {
        sys.foc_controller.ele_rad += _2PI;
    }
}

//    if((sys.foc_controller.angle - last_angle) > 270.0f)
//    {
//        rotation -= 1.0f;
//    }
//    else if((sys.foc_controller.angle - last_angle) < -270.0f)
//    {
//        rotation += 1.0f;
//    }
//    last_angle = sys.foc_controller.angle;
//    
//    sys.foc_controller.position = rotation * 360.f + sys.foc_controller.angle;

